from geometry_msgs.msg import PoseStamped
from nav2_simple_commander.robot_navigator import BasicNavigator
import rclpy

def main():
    rclpy.init()
    nav=BasicNavigator()
    nav.waitUntilNav2Active()#等待导航激活可用
    goal_pose=PoseStamped()
    goal_pose.header.frame_id="map"
    goal_pose.header.stamp=nav.get_clock().now().to_msg()
    while (1):  
        print("请输入目标点的坐标")
        goal_pose.pose.position.x=float(input("x: "))
        goal_pose.pose.position.y=float(input("y: "))
        goal_pose.pose.orientation.w=1.0#姿态（旋转），为1代表无旋转
        nav.goToPose(goal_pose)
        while not nav.isTaskComplete():
            feedback =nav.getFeedback()
            nav.get_logger().info(f'剩余距离：{feedback.distance_remaining}')
            # nav.cancelTask()   # 超时取消
        result=nav.getResult()
        nav.get_logger().info(f'导航结果：{result}')
        
    # rclpy.spin(nav)
    # rclpy.shutdown()